package Detour

import (
	"math"
	"unsafe"
)

type DTNodeIndex uint16

func DTHashRef(a DTPolyRef) uint {
	a = (^a) + (a << 18)
	a = a ^ (a >> 31)
	a = a * 21
	a = a ^ (a >> 11)
	a = a + (a << 6)
	a = a ^ (a >> 22)

	return uint(a)
}

const (
	DT_NODE_OPEN   = byte(0x01)
	DT_NODE_CLOSED = byte(0x02)
	DT_NULL_IDX    = math.MaxUint16
)

type DTNodePool struct {
	M_Nodes     []DTNode
	M_First     []DTNodeIndex
	M_Next      []DTNodeIndex
	M_MaxNodes  int
	M_HashSize  int
	M_NodeCount int
}

//NewDTNodePool///////////////////////////////////////////////
func NewDTNodePool(maxNodes int, hashSize int) *DTNodePool {
	newPool := &DTNodePool{}
	newPool.M_MaxNodes = maxNodes
	newPool.M_HashSize = hashSize

	if !(DTNextPow2(uint(newPool.M_HashSize)) == uint(newPool.M_HashSize)) {
		panic("NewDTNodePool")
	}
	if !(newPool.M_MaxNodes > 0) {
		panic("NewDTNodePool")
	}

	newPool.M_Nodes = make([]DTNode, newPool.M_MaxNodes)
	newPool.M_Next = make([]DTNodeIndex, newPool.M_MaxNodes)
	newPool.M_First = make([]DTNodeIndex, hashSize)

	if newPool.M_Nodes == nil {
		panic("newPool.M_Nodes Nil")
	}
	if newPool.M_Next == nil {
		panic("newPool.M_Nodes Nil")
	}
	if newPool.M_First == nil {
		panic("newPool.M_Nodes Nil")
	}

	for i := 0; i < hashSize; i++ {
		newPool.M_First[i] = DT_NULL_IDX
	}

	for i := 0; i < newPool.M_MaxNodes; i++ {
		newPool.M_Next[i] = DT_NULL_IDX
	}

	return newPool
}

func (this *DTNodePool) Clear() {
	for i := 0; i < this.M_HashSize; i++ {
		this.M_First[i] = DT_NULL_IDX
	}
	this.M_NodeCount = 0
}

func (this *DTNodePool) GetNodeIdx(node *DTNode) uint {
	if node == nil {
		return 0
	}
	return uint(NodeIndexOf(this.M_Nodes, node) + 1)
}

func (this *DTNodePool) GetNodeAtIdx(idx uint) *DTNode {
	if idx == 0 {
		return nil
	}
	return &this.M_Nodes[idx-1]
}

func (this *DTNodePool) GetMemUsed() int {
	var nint int
	var nindex DTNodeIndex
	return int(unsafe.Sizeof(nint))*3 + DTNodeGetSizeOf()*this.M_MaxNodes +
		int(unsafe.Sizeof(nindex))*this.M_MaxNodes + int(unsafe.Sizeof(nindex))*this.M_HashSize
}

func (this *DTNodePool) getMaxNodes() int {
	return this.M_MaxNodes
}

func (this *DTNodePool) getHashSize() int {
	return this.M_HashSize
}

func (this *DTNodePool) getFirst(bucket int) DTNodeIndex {
	return this.M_First[bucket]
}

func (this *DTNodePool) getNext(i int) DTNodeIndex {
	return this.M_Next[i]
}

func (this *DTNodePool) FindNode(id DTPolyRef) *DTNode {
	bucket := uint(DTHashRef(id)) & uint(this.M_HashSize-1)
	i := this.M_First[bucket]
	for i != DT_NULL_IDX {
		if this.M_Nodes[i].Id == id {
			return &this.M_Nodes[i]
		}
		i = this.M_Next[i]
	}
	return nil
}

func (this *DTNodePool) GetNode(id DTPolyRef) *DTNode {
	bucket := uint(DTHashRef(id)) & uint(this.M_HashSize-1)
	i := this.M_First[bucket]
	var node *DTNode
	for i != DT_NULL_IDX {
		if this.M_Nodes[i].Id == id {
			return &this.M_Nodes[i]
		}
		i = this.M_Next[i]
	}
	if this.M_NodeCount >= this.M_MaxNodes {
		return nil
	}
	i = DTNodeIndex(this.M_NodeCount)
	this.M_NodeCount++
	//Init node
	node = &this.M_Nodes[i]
	node.Pidx = 0
	node.Cost = 0
	node.Total = 0
	node.Id = id
	node.Flags = 0
	node.Pos = make([]float64, 3)

	this.M_Next[i] = this.M_First[bucket]
	this.M_First[bucket] = i
	return node
}

func NodeIndexOf(nodes []DTNode, node *DTNode) int {
	for i := 0; i < len(nodes); i++ {
		if &nodes[i] == node {
			return i
		}
	}
	return -1
}

///DTNode////////////////////////////////////////////
type DTNode struct {
	Pos   []float64
	Cost  float64
	Total float64
	Pidx  uint
	Flags byte
	Id    DTPolyRef
}

func DTNodeGetSizeOf() int {
	var f64 float64
	var nuint uint
	var nbyte byte
	var ndtpoly DTPolyRef
	return (int(unsafe.Sizeof(f64)) * (3 + 1 + 1)) + int(unsafe.Sizeof(nuint)) + int(unsafe.Sizeof(nbyte)) + int(unsafe.Sizeof(ndtpoly))

}

///dtNodeQueue//////////////////////////////////////////////////////////
type DTNodeQueue struct {
	M_Heap     []*DTNode
	M_Capacity int
	M_Size     int
}

func NewDTNodeQueue(n int) *DTNodeQueue {
	queue := &DTNodeQueue{}
	queue.M_Capacity = n
	queue.M_Heap = make([]*DTNode, queue.M_Capacity+1)
	return queue
}

func (this *DTNodeQueue) Clear() {
	this.M_Size = 0
}

func (this *DTNodeQueue) Top() *DTNode {
	return this.M_Heap[0]
}

func (this *DTNodeQueue) pop() *DTNode {
	result := this.M_Heap[0]
	this.M_Size--
	this.trickleDown(0, this.M_Heap[this.M_Size])
	return result
}

func (this *DTNodeQueue) push(node *DTNode) {
	this.M_Size++
	this.bubbleUp(this.M_Size-1, node)
}

func (this *DTNodeQueue) modify(node *DTNode) {
	for i := 0; i < this.M_Size; i++ {
		if this.M_Heap[i] == node {
			this.bubbleUp(i, node)
			return
		}
	}
}

func (this *DTNodeQueue) Empty() bool {
	return this.M_Size == 0
}

func (this *DTNodeQueue) GetMemUsed() int {
	var nint int
	return int(unsafe.Sizeof(nint)*2) + DTNodeGetSizeOf()*(this.M_Capacity+1)
}

func (this *DTNodeQueue) GetCapacity() int {
	return this.M_Capacity
}

func (this *DTNodeQueue) trickleDown(i int, node *DTNode) {
	child := (i * 2) + 1
	for child < this.M_Size {
		if (child+1) < this.M_Size && this.M_Heap[child].Total > this.M_Heap[child+1].Total {
			child++
		}
		this.M_Heap[i] = this.M_Heap[child]
		i = child
		child = (i * 2) + 1
	}
	this.bubbleUp(i, node)
}

func (this *DTNodeQueue) bubbleUp(i int, node *DTNode) {
	parent := (i - 1) / 2
	for (i > 0) && (this.M_Heap[parent].Total > node.Total) {
		this.M_Heap[i] = this.M_Heap[parent]
		i = parent
		parent = (i - 1) / 2
	}
	this.M_Heap[i] = node
}
